/*
 * @Author: wangyingjie 2778809626@qq.com
 * @Date: 2025-07-03 15:35:52
 * @LastEditors: wangyingjie 2778809626@qq.com
 * @LastEditTime: 2025-07-18 13:46:23
 * @FilePath: /two_wheeled_car/src/bot_nav/src/navigation_node.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <iostream>

// 通过动作客户端发送导航目标，并在RViz中显示目标点的可视化标记

class NavigationNode : public rclcpp::Node
{
public:
    using NavigateToPose = nav2_msgs::action::NavigateToPose;
    using GoalHandleNav = rclcpp_action::ClientGoalHandle<NavigateToPose>;

    NavigationNode() : Node("navigation_node")
    {
        // 创建一个NavigateToPose动作的客户端
        goal_client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
        // 创建一个发布可视化标记的发布者
        marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("visualization_marker", 10);

        set_initial_pose(); // 设置初始姿势
        prompt_new_goal();  // 提示输入新目标
    }

private:
    rclcpp_action::Client<NavigateToPose>::SharedPtr goal_client_;  // 用于发送导航目标
    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_; // 发布者的共享指针，用于发布可视化标记

    // 设置机器人的初始位置
    // 该函数用于在仿真或特定应用场景中设置机器人的初始位置
    // 在实际应用中，应使用 /initialpose 话题来设置初始位置
    void set_initial_pose()
    {
        // 输出日志信息，表示开始设置初始位置
        RCLCPP_INFO(this->get_logger(), "Setting initial pose...");
        
        // 模拟设置初始位置（实际应用中应通过 /initialpose 话题进行设置）
    }

    /**
     * @brief 发送目标点到导航系统
     * 
     * 该函数通过动作客户端发送一个目标点到导航系统，目标点包括位置(x, y)和方向(yaw)。
     * 如果导航系统在指定时间内没有响应，将输出错误信息并返回。否则，将构造目标消息并发送。
     * 
     * @param x 目标点的x坐标
     * @param y 目标点的y坐标
     * @param yaw 目标点的方向，以弧度表示
     */
    void send_goal(float x, float y, float yaw)
    {
        // 检查动作服务器是否可用，等待时间最长为5秒
        if (!goal_client_->wait_for_action_server(std::chrono::seconds(5)))
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available!");
            return;
        }
    
        // 构造目标消息
        auto goal_msg = NavigateToPose::Goal();
        goal_msg.pose.header.frame_id = "map";
        goal_msg.pose.header.stamp = this->get_clock()->now();
        goal_msg.pose.pose.position.x = x;
        goal_msg.pose.pose.position.y = y;
        goal_msg.pose.pose.orientation.w = cos(yaw / 2);
        goal_msg.pose.pose.orientation.z = sin(yaw / 2);
    
        // 配置发送目标的选项，包括结果回调函数
        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        send_goal_options.result_callback = [this, x, y](const GoalHandleNav::WrappedResult &result)
        {
            // 根据导航结果更新日志和标记
            if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
            {
                RCLCPP_INFO(this->get_logger(), "Goal reached!");
                add_marker(x, y);
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "Goal failed!");
            }
            // 提示用户设置新目标
            prompt_new_goal();
        };
    
        // 异步发送目标
        goal_client_->async_send_goal(goal_msg, send_goal_options);
    }

    /**
     * 添加一个可视化标记到RViz中。
     * 
     * 此函数创建并发送一个可视化标记消息，用于在RViz中显示一个红色的球形标记。
     * 标记的位置由x和y参数指定，位于地图坐标系中。
     * 
     * @param x 标记在地图坐标系中的x坐标。
     * @param y 标记在地图坐标系中的y坐标。
     */
    void add_marker(float x, float y)
    {
        // 创建一个Marker对象。
        visualization_msgs::msg::Marker marker;
        // 设置标记的参考坐标系为"map"。
        marker.header.frame_id = "map";
        // 设置标记的时间戳为当前时间。
        marker.header.stamp = this->get_clock()->now();
        // 设置标记的命名空间，用于区分不同的标记。
        marker.ns = "goal_marker";
        // 设置标记的唯一标识符。
        marker.id = 0;
        // 设置标记的类型为球形。
        marker.type = visualization_msgs::msg::Marker::SPHERE;
        // 设置标记的操作为添加。
        marker.action = visualization_msgs::msg::Marker::ADD;
        // 设置标记的位置。
        marker.pose.position.x = x;
        marker.pose.position.y = y;
        marker.pose.position.z = 0.2;
        // 设置标记的尺寸。
        marker.scale.x = 0.3;
        marker.scale.y = 0.3;
        marker.scale.z = 0.3;
        // 设置标记的颜色为红色，完全不透明。
        marker.color.r = 1.0;
        marker.color.g = 0.0;
        marker.color.b = 0.0;
        marker.color.a = 1.0;
        // 发布标记消息。
        marker_pub_->publish(marker);
    }

    void prompt_new_goal()
    {
        float x, y, yaw;
        std::cout << "Enter new goal (x, y, yaw): ";
        std::cin >> x >> y >> yaw;
        send_goal(x, y, yaw);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<NavigationNode>());
    rclcpp::shutdown();
    return 0;
}
